/* USER CODE BEGIN Header */
/**
  ******************************************************************************
  * @file           : main.c
  * @brief          : Main program body
  ******************************************************************************
  * @attention
  *
  * <h2><center>&copy; Copyright (c) 2021 STMicroelectronics.
  * All rights reserved.</center></h2>
  *
  * This software component is licensed by ST under BSD 3-Clause license,
  * the "License"; You may not use this file except in compliance with the
  * License. You may obtain a copy of the License at:
  *                        opensource.org/licenses/BSD-3-Clause
  *
  ******************************************************************************
  */
/* USER CODE END Header */
/* Includes ------------------------------------------------------------------*/
#include "main.h"
#include "i2c.h"
#include "tim.h"
#include "usart.h"
#include "gpio.h"

/* Private includes ----------------------------------------------------------*/
/* USER CODE BEGIN Includes */

//#include "iostream"
//#include<cstdio>
//#include<cstring>
//#include<queue>

#define COL 10
#define ROW 10
#define BOOL int
#define TRUE 1
#define FALSE 0






typedef struct plot
{
        int col;
        int row;
}Plot;

//迷宫用二维数组来存储，Maze[1][0]为起点，Maze[6][7]为终点，值为1表示�?????????
int Maze[COL][ROW] = {                  {0,0,0,0,0,0,0,0,0,0},
                                        {0,0,0,0,0,0,0,0,0,0},
                                        {0,0,0,0,0,0,0,0,0,0},
                                        {0,0,0,1,1,1,1,0,0,0},
                                        {0,0,0,1,1,1,1,0,0,0},
                                        {0,0,0,1,1,1,1,0,0,0},
                                        {0,0,0,1,1,1,1,0,0,0},
                                        {0,0,0,0,0,0,0,0,0,0},
                                        {0,0,0,0,0,0,0,0,0,0},
                                        {0,0,0,0,0,0,0,0,0,0}};
int head = 0, rear = 0;
int level = 2;
Plot flag_queue[COL * ROW];

void get_path(int x, int y);
void width_flag(int x, int y);
void en_queue(int x, int y);
void out_queue();
BOOL queue_not_empty();
void print_path();
void print_maze();


#include <string.h>
#include <stdlib.h>

int Maze2[COL][ROW] = {                  {0,0,0,0,0,0,0,0,0,0},
                                        {0,0,0,0,0,0,0,0,0,0},
                                        {0,0,0,0,0,0,0,0,0,0},
                                        {0,0,0,1,1,1,1,0,0,0},
                                        {0,0,0,1,1,1,1,0,0,0},
                                        {0,0,0,1,1,1,1,0,0,0},
                                        {0,0,0,1,1,1,1,0,0,0},
                                        {0,0,0,0,0,0,0,0,0,0},
                                        {0,0,0,0,0,0,0,0,0,0},
                                        {0,0,0,0,0,0,0,0,0,0}};



/* USER CODE END Includes */

/* Private typedef -----------------------------------------------------------*/
/* USER CODE BEGIN PTD */

/* USER CODE END PTD */

/* Private define ------------------------------------------------------------*/
/* USER CODE BEGIN PD */

//定义循迹
char xj2_0;
char xj2_1;
char xj2_2;
char xj2_3;
char xj2_4;
char xj2_5;
char xj2_6;
char xj2_7;

char xj4[8];



int count2;
int count4;




//jetson nano
uint8_t  jetson_data[17];

extern char class[6];


//循迹
const int weight1[8] = {-16,-8,-4,-2,2,4,8,16};
int speed = 250;
int turn_speed = 300;
int speed_L = 0, speed_R=0;
int error = 0;
int KP = 2;

int count = 0;

int top_line = 0;


//编码器中位默认�??
#define MID_VALUE 10000


/* 电机旋转方向 */
__IO int8_t Motor_Direction = 0;
/* 当前时刻总计数�?? */
__IO int32_t Capture_Count = 0;
/* 上一时刻总计数�?? */
__IO int32_t Last_Count = 0;
/* 电机转轴转�?? */
__IO float Shaft_Speed = 0.0f;



int led_num = 0;    //前循迹模块亮灯个�?????????



//地图map
int now_xx = 0;
int now_yy = 0;
int now_dirr = 0;

int des_x = 0;
int des_y = 0;
int des_dir = 0;

int map_array[30];



void get_data();
void go_stright();
void go_stright_back();
void go_left();
void go_right();
void go_back();
void go_stright_num(int num);
void go_map(int des_x,int des_y,int des_dir);
void go_map1(int now_x,int now_y,int now_dir,int des_x,int des_y,int des_dir);


#include "stdio.h"
#include "LobotServoController.h"
#ifdef __GNUC__

#define PUTCHAR_PROTOTYPE int __io_putchar(int ch)

PUTCHAR_PROTOTYPE
{

  HAL_UART_Transmit(&huart3, (uint8_t*)&ch, 1, HAL_MAX_DELAY);
  return ch;
}
#endif





/* USER CODE END PD */

/* Private macro -------------------------------------------------------------*/
/* USER CODE BEGIN PM */

/* USER CODE END PM */

/* Private variables ---------------------------------------------------------*/

/* USER CODE BEGIN PV */

/* USER CODE END PV */

/* Private function prototypes -----------------------------------------------*/
void SystemClock_Config(void);
/* USER CODE BEGIN PFP */

/* USER CODE END PFP */

/* Private user code ---------------------------------------------------------*/
/* USER CODE BEGIN 0 */


/* USER CODE BEGIN 1 */

/* USER CODE END 1 */






/* USER CODE END 0 */

/**
  * @brief  The application entry point.
  * @retval int
  */
int main(void)
{
  /* USER CODE BEGIN 1 */

  /* USER CODE END 1 */

  /* MCU Configuration--------------------------------------------------------*/

  /* Reset of all peripherals, Initializes the Flash interface and the Systick. */
  HAL_Init();

  /* USER CODE BEGIN Init */


  /* USER CODE END Init */

  /* Configure the system clock */
  SystemClock_Config();

  /* USER CODE BEGIN SysInit */
	MX_GPIO_Init();
	MX_TIM3_Init();
	MX_USART3_UART_Init();
	MX_TIM2_Init();
	MX_TIM4_Init();


  /* USER CODE END SysInit */

  /* Initialize all configured peripherals */
  MX_GPIO_Init();
  MX_TIM3_Init();
  MX_USART3_UART_Init();
  MX_TIM2_Init();
  MX_TIM4_Init();
  MX_USART1_UART_Init();
  MX_I2C2_Init();
  MX_USART2_UART_Init();
  /* USER CODE BEGIN 2 */
  	HAL_TIM_PWM_Start(&htim3,TIM_CHANNEL_1);
  	__HAL_TIM_SET_COMPARE(&htim3,TIM_CHANNEL_1,0);

  	HAL_TIM_PWM_Start(&htim3,TIM_CHANNEL_2);
  	__HAL_TIM_SET_COMPARE(&htim3,TIM_CHANNEL_2,0);

  	HAL_TIM_PWM_Start(&htim3,TIM_CHANNEL_3);
  	__HAL_TIM_SET_COMPARE(&htim3,TIM_CHANNEL_3,0);

  	HAL_TIM_PWM_Start(&htim3,TIM_CHANNEL_4);
  	__HAL_TIM_SET_COMPARE(&htim3,TIM_CHANNEL_4,0);

  	HAL_TIM_Encoder_Start(&htim2, TIM_CHANNEL_ALL);
  	HAL_TIM_Encoder_Start(&htim4, TIM_CHANNEL_ALL);

  //电机123方向IO 都设 制动
  HAL_GPIO_WritePin(GPIOG,GPIO_PIN_12,GPIO_PIN_RESET);
  HAL_GPIO_WritePin(GPIOG,GPIO_PIN_11,GPIO_PIN_RESET);

  HAL_GPIO_WritePin(GPIOG,GPIO_PIN_14,GPIO_PIN_RESET);
  HAL_GPIO_WritePin(GPIOG,GPIO_PIN_13,GPIO_PIN_RESET);

  HAL_GPIO_WritePin(GPIOG,GPIO_PIN_10,GPIO_PIN_RESET);
  HAL_GPIO_WritePin(GPIOD,GPIO_PIN_5,GPIO_PIN_RESET);

  HAL_GPIO_WritePin(GPIOD,GPIO_PIN_7,GPIO_PIN_RESET);
  HAL_GPIO_WritePin(GPIOD,GPIO_PIN_6,GPIO_PIN_RESET);


  /* USER CODE END 2 */

  /* Infinite loop */
  /* USER CODE BEGIN WHILE */



  //编码器复�?????????
  __HAL_TIM_SET_COUNTER(&htim4,MID_VALUE);    //�?????????      前进减数
  __HAL_TIM_SET_COUNTER(&htim2,MID_VALUE);	  //�?????????      前进加数







  //jetson nano usart1
  HAL_UART_Receive_IT(&huart1,jetson_data,14);
  HAL_UART_Receive_IT(&huart3,jetson_data,17);








//  while(1)
//  {
//
//
//	  printf("1234\r\n");
//
//	  HAL_Delay(1000);
//  }



  //机械臂程�???
//5舵机  大于1500
//  while(1)
//  {
//
//
//	  runActionGroup(0,1);
//	  HAL_Delay(2000);
//
//	  runActionGroup(1,1);
//	  HAL_Delay(2000);
//
//
//
////	  moveServo(4, 1500, 1000);
//
////	  printf("112233\r\n");
////	  runActionGroup(4,2);
////	  moveServo(5,1800,200);    //单个舵机
////
////
//////	  MOTOR_PWM(2,0);
//////	  MOTOR_PWM(3,0);
//////
////	  HAL_Delay(2000);
////	  moveServo(5,1500,200);
////	  HAL_Delay(2000);
////
////	  HAL_GPIO_TogglePin(GPIOB,GPIO_PIN_8);
//
//
////	  MOTOR_STOP();
////	  HAL_Delay(2000);
//  }












  speed = 500;

//  go_stright_num(1);
//
//  MOTOR_PWM(2,0);
//  MOTOR_PWM(3,0);
//
//  HAL_Delay(1000);
//
//  go_left();
//
//
//MOTOR_PWM(2,0);
//MOTOR_PWM(3,0);
//
//
//
//HAL_Delay(1000);

//  while(1)
//  {
//
////	  go_stright_num(1);
////
////	  go_right();
//	  MOTOR_PWM(2,300);
////	  MOTOR_PWM(3,300);
//
//
//	  HAL_Delay(3000);
//
//	  MOTOR_PWM(2,-300);
////	  MOTOR_PWM(3,-300);
//
//
//	  HAL_Delay(3000);
//
//
//
//
//
//
//  }
//  go_map1(now_xx,now_yy,now_dirr,1,1,0);
  //(0,2)(0,4)(0,8)(4,9)(5,9)(8,9)(9,5)(9,2)(9,1)(4,0)(2,0)
  //(4,2)(7,4)(5,7)(2,6)(0,2)



//  //抓取
//  __HAL_TIM_SET_COUNTER(&htim4,MID_VALUE);
//  __HAL_TIM_SET_COUNTER(&htim2,MID_VALUE);
//
//  while(1)
//  {
//
//
//
//	  int count2 = __HAL_TIM_GET_COUNTER(&htim2);
//	  int count4 = __HAL_TIM_GET_COUNTER(&htim4);
//
//
//
//	  printf("%d,%d\r\n",count2,count4);
//	  HAL_Delay(400);
//  }






//复位
  runActionGroup(0,1);

//  HAL_Delay(3000);
//
//
//
//  runActionGroup(1,1);
//  HAL_Delay(4000);
//  moveServo(5, 1650, 700);
//  HAL_Delay(3000);
//  runActionGroup(11,1);


    __HAL_TIM_SET_COUNTER(&htim4,MID_VALUE);
    __HAL_TIM_SET_COUNTER(&htim2,MID_VALUE);

  speed = 300;


//  go_stright_back();
//
////  MOTOR_PWM(2,-200);
////  MOTOR_PWM(3,-200);
//





  go_stright_num(1);
  MOTOR_PWM(2,0);
  MOTOR_PWM(3,0);

  HAL_Delay(1000);

  go_left();
  HAL_Delay(1000);


  go_stright_num(1);//转向之后直行1   判断为顶停止。


  int count2 = __HAL_TIM_GET_COUNTER(&htim2);
  int count4 = __HAL_TIM_GET_COUNTER(&htim4);
  while(abs(count2 - MID_VALUE)<200 || abs(count4 - MID_VALUE)<200)    //前进到旋转的位置
  {

  	count2 = __HAL_TIM_GET_COUNTER(&htim2);
  	count4 = __HAL_TIM_GET_COUNTER(&htim4);



  	//=====================循迹===========
  	get_data();     //刷新循迹数据

  	go_stright();

  }



  MOTOR_PWM(2,0);
  MOTOR_PWM(3,0);




  HAL_Delay(1000);

  //操作循迹模块
  while(1)
  {
	  get_data();

	  printf("%d %d %d %d %d %d %d %d\r\n",xj2_0,xj2_1,xj2_2,xj2_3,xj2_4,xj2_5,xj2_6,xj2_7);

	  error = xj2_0*weight1[0]+xj2_1*weight1[1]+xj2_2*weight1[2]+xj2_3*weight1[3]+xj2_4*weight1[4]+xj2_5*weight1[5]+xj2_6*weight1[6]+xj2_7*weight1[7];


//	  speed_R = -error;
//	  speed_L =  error;
//	//  printf("L :%d   R:%d\r\n",speed_R,speed_L);
//
//
//	  MOTOR_PWM(2,10*speed_L);    //左右差�?�控制方�?????????
//	  MOTOR_PWM(3,10*speed_R);    //2号为左电 3为右电机
//
	  if(error>0)
	  {
		  MOTOR_PWM(2,100);    //
		  MOTOR_PWM(3,-100);    //2号为左电 3为右电机
	  }

	  if(error<0)
	  {
		  MOTOR_PWM(2,-100);    //
		  MOTOR_PWM(3,100);    //2号为左电 3为右电机
	  }

	  if(error==0)
	  {
		  MOTOR_PWM(2,0);    //
		  MOTOR_PWM(3,0);    //2号为左电 3为右电机
		  break;
	  }

  }














  while(abs(count2 - MID_VALUE)>10 || abs(count4 - MID_VALUE)>10)    //倒退到旋转的位置
  {


  	count2 = __HAL_TIM_GET_COUNTER(&htim2);
  	count4 = __HAL_TIM_GET_COUNTER(&htim4);



  	//=====================循迹===========
  	get_data();     //刷新循迹数据

  	go_stright_back();

  }


  MOTOR_PWM(2,0);
  MOTOR_PWM(3,0);


  HAL_Delay(1000);









  //倒退出去        循迹退出顶线   num < 4     再编码器走直线一段距离       左转右转
  get_data();
  led_num = xj2_0 + xj2_1 + xj2_2 + xj2_3 + xj2_4 + xj2_5 + xj2_6 + xj2_7;

  __HAL_TIM_SET_COUNTER(&htim4,MID_VALUE);
  __HAL_TIM_SET_COUNTER(&htim2,MID_VALUE);
  while(led_num<=4)    //不在白线上
  {
	  get_data();
	  led_num = xj2_0 + xj2_1 + xj2_2 + xj2_3 + xj2_4 + xj2_5 + xj2_6 + xj2_7;

	  count2 = __HAL_TIM_GET_COUNTER(&htim2);
	  count4 = __HAL_TIM_GET_COUNTER(&htim4);

	  int error = abs(MID_VALUE-count2) - abs(MID_VALUE-count4);

	  speed_R = -200 + 10*error;
	  speed_L = -200 - 10*error;
	//  printf("L :%d   R:%d\r\n",speed_R,speed_L);


	  MOTOR_PWM(2,speed_L);    //左右差�?�控制方�?????????
	  MOTOR_PWM(3,speed_R);    //2号为左电 3为右电机



  }

  __HAL_TIM_SET_COUNTER(&htim4,MID_VALUE);
  __HAL_TIM_SET_COUNTER(&htim2,MID_VALUE);
  while(led_num>4)    //还在白线上
  {
	  get_data();
	  led_num = xj2_0 + xj2_1 + xj2_2 + xj2_3 + xj2_4 + xj2_5 + xj2_6 + xj2_7;

	  count2 = __HAL_TIM_GET_COUNTER(&htim2);
	  count4 = __HAL_TIM_GET_COUNTER(&htim4);

	  int error = abs(MID_VALUE-count2) - abs(MID_VALUE-count4);

	  speed_R = -200 + 10*error;
	  speed_L = -200 - 10*error;
	//  printf("L :%d   R:%d\r\n",speed_R,speed_L);


	  MOTOR_PWM(2,speed_L);    //左右差�?�控制方�?????????
	  MOTOR_PWM(3,speed_R);    //2号为左电 3为右电机



  }


  MOTOR_PWM(2,0);
  MOTOR_PWM(3,0);


  HAL_Delay(1000);



    __HAL_TIM_SET_COUNTER(&htim4,MID_VALUE);
    __HAL_TIM_SET_COUNTER(&htim2,MID_VALUE);






    //倒退100 ，左轮后退



    __HAL_TIM_SET_COUNTER(&htim4,MID_VALUE);
    __HAL_TIM_SET_COUNTER(&htim2,MID_VALUE);
	  count2 = __HAL_TIM_GET_COUNTER(&htim2);
	  count4 = __HAL_TIM_GET_COUNTER(&htim4);
	  int pos = 100;
    while( abs(count2 - MID_VALUE)<pos || abs(count4 - MID_VALUE)<pos )    //
    {
  	  get_data();
  	  led_num = xj2_0 + xj2_1 + xj2_2 + xj2_3 + xj2_4 + xj2_5 + xj2_6 + xj2_7;

  	  count2 = __HAL_TIM_GET_COUNTER(&htim2);
  	  count4 = __HAL_TIM_GET_COUNTER(&htim4);

  	  int error = abs(MID_VALUE-count2) - abs(MID_VALUE-count4);

  	  speed_R = -200 + 10*error;
  	  speed_L = -200 - 10*error;
  	//  printf("L :%d   R:%d\r\n",speed_R,speed_L);


  	  MOTOR_PWM(2,speed_L);    //左右差�?�控制方�?????????
  	  MOTOR_PWM(3,speed_R);    //2号为左电 3为右电机



    }



#if 0    //111    AB继续走
    //开始倒回去



    //编码器复�?????????
    	  __HAL_TIM_SET_COUNTER(&htim4,MID_VALUE);    //�?????????      前进减数
    	  __HAL_TIM_SET_COUNTER(&htim2,MID_VALUE);	  //�?????????      前进加数


    	  MOTOR_PWM(2,0);
    	  MOTOR_PWM(3,-turn_speed);      //左轮�?????????  右轮�?????????

    	  count2 = __HAL_TIM_GET_COUNTER(&htim2);
    	  count4 = __HAL_TIM_GET_COUNTER(&htim4);


    	  while(abs(count2 - MID_VALUE)<500)    //前进到旋转的位置
    	  {

    		  //get_data();    //刷新循迹数据
    		  count2 = __HAL_TIM_GET_COUNTER(&htim2);
    		  count4 = __HAL_TIM_GET_COUNTER(&htim4);
    //		  printf("c2 = %d     c4 = %d\r\n",count2,count4);


    	  }


    //	  MOTOR_PWM(2,0);
    //	  MOTOR_PWM(3,0);
    //		  HAL_Delay(1000);

    	  MOTOR_PWM(2,0);
    	  MOTOR_PWM(3,-turn_speed);      //左轮�?????????  右轮�?????????


    	  int cnt=0;
    	  while(abs(count2 - MID_VALUE)<1100 /* && (xj2_3==0) && (xj2_4==0) */)    //前进到旋转的位置
    	  {

    		  get_data();    //刷新循迹数据



    		  if(xj2_3==1||xj2_4==1)
    		  {
    			  cnt++;

    		  }else
    		  {
    			  cnt = 0;
    		  }


    		  if(cnt>20)
    		  {
    			  break;
    		  }



    		  count2 = __HAL_TIM_GET_COUNTER(&htim2);
    		  count4 = __HAL_TIM_GET_COUNTER(&htim4);
    //		  printf("c2 = %d     c4 = %d\r\n",count2,count4);


    	  }




	  //旋转后的复位。   有bug
	  while(1)
	  {
		  get_data();

//		  printf("%d %d %d %d %d %d %d %d\r\n",xj2_0,xj2_1,xj2_2,xj2_3,xj2_4,xj2_5,xj2_6,xj2_7);

		  error = xj2_0*weight1[0]+xj2_1*weight1[1]+xj2_2*weight1[2]+xj2_3*weight1[3]+xj2_4*weight1[4]+xj2_5*weight1[5]+xj2_6*weight1[6]+xj2_7*weight1[7];


	//	  speed_R = -error;
	//	  speed_L =  error;
	//	//  printf("L :%d   R:%d\r\n",speed_R,speed_L);
	//
	//
	//	  MOTOR_PWM(2,10*speed_L);    //左右差�?�控制方�?????????
	//	  MOTOR_PWM(3,10*speed_R);    //2号为左电 3为右电机
	//
		  if(error>0)
		  {
			  MOTOR_PWM(2,200);    //
			  MOTOR_PWM(3,-200);    //2号为左电 3为右电机
		  }

		  if(error<0)
		  {
			  MOTOR_PWM(2,-200);    //
			  MOTOR_PWM(3,200);    //2号为左电 3为右电机
		  }

		  if(error==0)
		  {
			  MOTOR_PWM(2,0);    //
			  MOTOR_PWM(3,0);    //2号为左电 3为右电机
			  break;
		  }

	  }





#endif


#if 1//AB往回走

	  //编码器复�?????????
	  __HAL_TIM_SET_COUNTER(&htim4,MID_VALUE);    //�?????????      前进减数
	  __HAL_TIM_SET_COUNTER(&htim2,MID_VALUE);	  //�?????????      前进加数


	  MOTOR_PWM(2,-turn_speed);
	  MOTOR_PWM(3,0);      //右轮�?????????  左轮�?????????

	  count2 = __HAL_TIM_GET_COUNTER(&htim2);
	  count4 = __HAL_TIM_GET_COUNTER(&htim4);



	  while(abs(count4 - MID_VALUE)<500)    //前进到旋转的位置
	  {

		  //get_data();    //刷新循迹数据
		  count2 = __HAL_TIM_GET_COUNTER(&htim2);
		  count4 = __HAL_TIM_GET_COUNTER(&htim4);
//		  printf("c2 = %d     c4 = %d\r\n",count2,count4);


	  }

//	  MOTOR_PWM(2,0);
//	  MOTOR_PWM(3,0);
//		  HAL_Delay(1000);

	  MOTOR_PWM(2,-turn_speed);
	  MOTOR_PWM(3,0);      //左轮�?????????  右轮�?????????


	  int cnt=0;
	  while(abs(count4 - MID_VALUE)<1100 /* && (xj2_3==0) && (xj2_4==0) */)    //前进到旋转的位置
	  {

		  get_data();    //刷新循迹数据



		  if(xj2_3==1||xj2_4==1)
		  {
			  cnt++;

		  }else
		  {
			  cnt = 0;
		  }


		  if(cnt>20)
		  {
			  break;
		  }



		  count2 = __HAL_TIM_GET_COUNTER(&htim2);
		  count4 = __HAL_TIM_GET_COUNTER(&htim4);
//		  printf("c2 = %d     c4 = %d\r\n",count2,count4);


	  }

	  //旋转后的复位。   有bug
	  while(1)
	  {
		  get_data();

//		  printf("%d %d %d %d %d %d %d %d\r\n",xj2_0,xj2_1,xj2_2,xj2_3,xj2_4,xj2_5,xj2_6,xj2_7);

		  error = xj2_0*weight1[0]+xj2_1*weight1[1]+xj2_2*weight1[2]+xj2_3*weight1[3]+xj2_4*weight1[4]+xj2_5*weight1[5]+xj2_6*weight1[6]+xj2_7*weight1[7];


	//	  speed_R = -error;
	//	  speed_L =  error;
	//	//  printf("L :%d   R:%d\r\n",speed_R,speed_L);
	//
	//
	//	  MOTOR_PWM(2,10*speed_L);    //左右差�?�控制方�?????????
	//	  MOTOR_PWM(3,10*speed_R);    //2号为左电 3为右电机
	//
		  if(error>0)
		  {
			  MOTOR_PWM(2,200);    //
			  MOTOR_PWM(3,-200);    //2号为左电 3为右电机
		  }

		  if(error<0)
		  {
			  MOTOR_PWM(2,-200);    //
			  MOTOR_PWM(3,200);    //2号为左电 3为右电机
		  }

		  if(error==0)
		  {
			  MOTOR_PWM(2,0);    //
			  MOTOR_PWM(3,0);    //2号为左电 3为右电机
			  break;
		  }

	  }



#endif








	  MOTOR_PWM(2,0);
	  MOTOR_PWM(3,0);


	  HAL_Delay(1000);







    while(1)
    {



  	  int count2 = __HAL_TIM_GET_COUNTER(&htim2);
  	  int count4 = __HAL_TIM_GET_COUNTER(&htim4);



  	  printf("%d,%d\r\n",count2,count4);
  	  HAL_Delay(400);
    }








  //编码器前计数   姿态调整








int ii=0;




  while(1)
  {
	  int count2 = __HAL_TIM_GET_COUNTER(&htim2);
	  int count4 = __HAL_TIM_GET_COUNTER(&htim4);
	  printf("%d,%d\r\n",count2,count4);

	  ii++;
	  if(ii==7)
	  {
		  ii=1;
		  continue;
	  }

	  runActionGroup(0,1);

	  HAL_Delay(3000);

	  count2 = __HAL_TIM_GET_COUNTER(&htim2);
	  count4 = __HAL_TIM_GET_COUNTER(&htim4);
	  printf("%d,%d\r\n",count2,count4);



	  runActionGroup(ii,1);
	  HAL_Delay(4000);

	  count2 = __HAL_TIM_GET_COUNTER(&htim2);
	  count4 = __HAL_TIM_GET_COUNTER(&htim4);
	  printf("%d,%d\r\n",count2,count4);


	  moveServo(5, 1650, 700);
	  HAL_Delay(3000);

	  count2 = __HAL_TIM_GET_COUNTER(&htim2);
	  count4 = __HAL_TIM_GET_COUNTER(&htim4);
	  printf("%d,%d\r\n",count2,count4);

	  runActionGroup(ii+10,1);
	  HAL_Delay(4000);
	  //松掉
	  moveServo(5, 2000, 700);
	  HAL_Delay(1000);


  }





  now_xx  = 0;
  now_yy  = 1;
  now_dirr = 0;

  speed = 350;
  go_stright_num(2);



  MOTOR_PWM(2,0);
  MOTOR_PWM(3,0);



  runActionGroup(1,1);
  moveServo(5, 1650, 200);
  runActionGroup(11,1);







  while(1)
  {

  }




  int delay = 3000;

  while(1)
  {

	  speed = 350;

//	  go_map1(now_xx,now_yy,now_dirr,0,2,1);
//	  MOTOR_PWM(2,0);
//	  MOTOR_PWM(3,0);
//	  HAL_Delay(delay);


	  go_map1(now_xx,now_yy,now_dirr,0,4,2);
	  MOTOR_PWM(2,0);
	  MOTOR_PWM(3,0);
	  HAL_Delay(delay);

	  go_map1(now_xx,now_yy,now_dirr,0,8,2);
	  MOTOR_PWM(2,0);
	  MOTOR_PWM(3,0);
	  HAL_Delay(delay);

	  go_map1(now_xx,now_yy,now_dirr,4,9,1);
	  MOTOR_PWM(2,0);
	  MOTOR_PWM(3,0);
	  HAL_Delay(delay);

	  go_map1(now_xx,now_yy,now_dirr,5,9,1);
	  MOTOR_PWM(2,0);
	  MOTOR_PWM(3,0);
	  HAL_Delay(delay);

	  go_map1(now_xx,now_yy,now_dirr,8,9,1);
	  MOTOR_PWM(2,0);
	  MOTOR_PWM(3,0);
	  HAL_Delay(delay);

	  go_map1(now_xx,now_yy,now_dirr,9,5,0);
	  MOTOR_PWM(2,0);
	  MOTOR_PWM(3,0);
	  HAL_Delay(delay);

	  go_map1(now_xx,now_yy,now_dirr,9,2,0);
	  MOTOR_PWM(2,0);
	  MOTOR_PWM(3,0);
	  HAL_Delay(delay);


	  go_map1(now_xx,now_yy,now_dirr,9,1,0);
	  MOTOR_PWM(2,0);
	  MOTOR_PWM(3,0);
	  HAL_Delay(delay);

	  go_map1(now_xx,now_yy,now_dirr,9,0,0);
	  MOTOR_PWM(2,0);
	  MOTOR_PWM(3,0);
//	  HAL_Delay(delay);


	  go_map1(now_xx,now_yy,now_dirr,4,0,3);
	  MOTOR_PWM(2,0);
	  MOTOR_PWM(3,0);
	  HAL_Delay(delay);


	  go_map1(now_xx,now_yy,now_dirr,2,0,3);
	  MOTOR_PWM(2,0);
	  MOTOR_PWM(3,0);
	  HAL_Delay(delay);

	  go_map1(now_xx,now_yy,now_dirr,4,2,1);
	  MOTOR_PWM(2,0);
	  MOTOR_PWM(3,0);
	  HAL_Delay(delay);

	  go_map1(now_xx,now_yy,now_dirr,7,4,2);
	  MOTOR_PWM(2,0);
	  MOTOR_PWM(3,0);
	  HAL_Delay(delay);

	  go_map1(now_xx,now_yy,now_dirr,5,7,3);
	  MOTOR_PWM(2,0);
	  MOTOR_PWM(3,0);
	  HAL_Delay(delay);

	  go_map1(now_xx,now_yy,now_dirr,2,6,0);
	  MOTOR_PWM(2,0);
	  MOTOR_PWM(3,0);
	  HAL_Delay(delay);

	  go_map1(now_xx,now_yy,now_dirr,1,2,0);
//	  MOTOR_PWM(2,0);
//	  MOTOR_PWM(3,0);
//	  HAL_Delay(delay);

	  go_map1(now_xx,now_yy,now_dirr,0,2,0);
	  MOTOR_PWM(2,0);
	  MOTOR_PWM(3,0);
	  HAL_Delay(3*delay);


	  printf("1234\r\n");


  }



  while (1)
  {


//	  //编码器复�?????????
//	  __HAL_TIM_SET_COUNTER(&htim4,MID_VALUE);    //�?????????      前进减数
//	  __HAL_TIM_SET_COUNTER(&htim2,MID_VALUE);	  //�?????????      前进加数
//
//
//
//	  go_map(7, 3, 0);
//
//

	  go_map1(now_xx,now_yy,now_dirr,1,1,0);

	  MOTOR_PWM(2,0);
	  MOTOR_PWM(3,0);

	  HAL_Delay(500);


	  go_map1(now_xx,now_yy,now_dirr,1,8,0);




	  MOTOR_PWM(2,0);
	  MOTOR_PWM(3,0);

	  HAL_Delay(500);

	  go_map1(now_xx,now_yy,now_dirr,8,8,0);




	  MOTOR_PWM(2,0);
	  MOTOR_PWM(3,0);

	  HAL_Delay(500);


	  go_map1(now_xx,now_yy,now_dirr,8,1,0);




	  MOTOR_PWM(2,0);
	  MOTOR_PWM(3,0);

	  HAL_Delay(500);




	  printf("nowx = %d  nowy = %d  now_dir = %d",now_xx,now_yy,now_dirr);


//
//      width_flag(4, 9);
//      get_path(4, 0);
//
//
//
//
//      print_path();
//      print_maze();
//
//      HAL_Delay(10000);




//	  go_stright_num(2);
//
//	  MOTOR_PWM(2,0);
//	  MOTOR_PWM(3,0);
//	  HAL_Delay(2000);
//
//
//	  go_left();


//	  go_stright_num(2);
//	  MOTOR_PWM(2,0);
//	  MOTOR_PWM(3,0);
//	  HAL_Delay(2000);








//	  while(1)
//	  {
//
//
//
//		  int count2 = __HAL_TIM_GET_COUNTER(&htim2);
//		  int count4 = __HAL_TIM_GET_COUNTER(&htim4);
//		  printf("c2 = %d     c4 = %d\r\n",count2,count4);
//
//		  HAL_Delay(100);
//
//
//	  }

//
//	  get_data();    //刷新循迹数据
//
//	  led_num = xj2_0 + xj2_1 + xj2_2 + xj2_3 + xj2_4 + xj2_5 + xj2_6 + xj2_7;      //前循�?????????  亮灯个数
//
//
//
//
//	  //printf("%d %d %d %d %d %d %d %d\r\n",xj2_0,xj2_1,xj2_2,xj2_3,xj2_4,xj2_5,xj2_6,xj2_7);
//
//
//	  //逻辑 :前端触线 后端循迹 (达到几个灯亮)   / 判断丢先 左丢�???????????????? 右丢�????????????????  回线  / 转向动作 编码�???????????????? /
//
//	  if(led_num < 4)
//	  {
//
//		  go_stright();    //走直�?????????
//
//	  }
//
//	  if(led_num >= 4  && count<20)   //可能到达顶线   不判断位�?????????
//	  {
//		  count ++;
//
//		  continue;
//
//	  }
//
//
//	  if(count>=20)   //判断为顶�?????????
//	  {
//		  count = 0;
//
//		  top_line = 1;
//	  }
//
//
//	  if(top_line==1)
//	  {
//		  top_line = 0;
//
//		  //go_left();
//		  go_back();
//
//	  }
//









//	  int count2 = __HAL_TIM_GET_COUNTER(&htim2);
//	  int count4 = __HAL_TIM_GET_COUNTER(&htim4);
//
//
//	  printf("c2 = %d     c4 = %d\r\n",count2,count4);
//




//	  if ((xj2_0 + xj2_1 + xj2_2 + xj2_3 + xj2_4 + xj2_5 + xj2_6 + xj2_7) > 4){
//		  count++;
//		  HAL_Delay(10);
//		  printf("conut = :%d\r\n",count);
//	  }
//	  if(count == 7){
//		  oldL = __HAL_TIM_GET_COUNTER(&htim4);
//		  oldR = __HAL_TIM_GET_COUNTER(&htim2);
//		  MOTOR_CONTROL(2, 0);
//		  MOTOR_CONTROL(3, 1);
//		  MOTOR_PWM(2,0);
//		  MOTOR_PWM(3,200);
//		  while(1){
//			  int flag_L = 1, flag_R = 0;
//			  CaptureNumberL =__HAL_TIM_GET_COUNTER(&htim4);
//			  CaptureNumberR =__HAL_TIM_GET_COUNTER(&htim2);
//
//
//			  printf("L:%ld\r\n",  (long int)CaptureNumberL);
//			  printf("R:%ld\r\n",   (long int)CaptureNumberR);
//			  if(abs(oldL - CaptureNumberL) >= 1000 && !flag_L){
//				  MOTOR_PWM(2,0);
//				  flag_L = 1;
//			  }
//
//			  if(abs(oldR - CaptureNumberR) >= 1000 && !flag_R ){
//				  MOTOR_PWM(3,0);
//				  flag_R = 1;
//			  }
//			  if(flag_L > 0 && flag_R > 0){
//				  break;
//			  }
//			}
//		  printf("stop");
//		  count = 0;
//	  }

	  HAL_GPIO_TogglePin(GPIOB,GPIO_PIN_8);
	  HAL_Delay(10);

  }


    /* USER CODE END WHILE */

    /* USER CODE BEGIN 3 */

  /* USER CODE END 3 */
}

/**
  * @brief System Clock Configuration
  * @retval None
  */
void SystemClock_Config(void)
{
  RCC_OscInitTypeDef RCC_OscInitStruct = {0};
  RCC_ClkInitTypeDef RCC_ClkInitStruct = {0};

  /** Initializes the RCC Oscillators according to the specified parameters
  * in the RCC_OscInitTypeDef structure.
  */
  RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_HSE;
  RCC_OscInitStruct.HSEState = RCC_HSE_ON;
  RCC_OscInitStruct.HSEPredivValue = RCC_HSE_PREDIV_DIV1;
  RCC_OscInitStruct.HSIState = RCC_HSI_ON;
  RCC_OscInitStruct.PLL.PLLState = RCC_PLL_ON;
  RCC_OscInitStruct.PLL.PLLSource = RCC_PLLSOURCE_HSE;
  RCC_OscInitStruct.PLL.PLLMUL = RCC_PLL_MUL9;
  if (HAL_RCC_OscConfig(&RCC_OscInitStruct) != HAL_OK)
  {
    Error_Handler();
  }
  /** Initializes the CPU, AHB and APB buses clocks
  */
  RCC_ClkInitStruct.ClockType = RCC_CLOCKTYPE_HCLK|RCC_CLOCKTYPE_SYSCLK
                              |RCC_CLOCKTYPE_PCLK1|RCC_CLOCKTYPE_PCLK2;
  RCC_ClkInitStruct.SYSCLKSource = RCC_SYSCLKSOURCE_PLLCLK;
  RCC_ClkInitStruct.AHBCLKDivider = RCC_SYSCLK_DIV1;
  RCC_ClkInitStruct.APB1CLKDivider = RCC_HCLK_DIV2;
  RCC_ClkInitStruct.APB2CLKDivider = RCC_HCLK_DIV1;

  if (HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_2) != HAL_OK)
  {
    Error_Handler();
  }
}

/* USER CODE BEGIN 4 */
void get_data()
{
	  xj2_0 = !HAL_GPIO_ReadPin(GPIOF,GPIO_PIN_13);
	  xj2_1 = !HAL_GPIO_ReadPin(GPIOG,GPIO_PIN_1);
	  xj2_2 = !HAL_GPIO_ReadPin(GPIOF,GPIO_PIN_14);
	  xj2_3 = !HAL_GPIO_ReadPin(GPIOE,GPIO_PIN_7);
	  xj2_4 = !HAL_GPIO_ReadPin(GPIOF,GPIO_PIN_15);
	  xj2_5 = !HAL_GPIO_ReadPin(GPIOF,GPIO_PIN_11);
	  xj2_6 = !HAL_GPIO_ReadPin(GPIOG,GPIO_PIN_0);
	  xj2_7 = !HAL_GPIO_ReadPin(GPIOF,GPIO_PIN_12);

	  xj4[0] = !HAL_GPIO_ReadPin(GPIOD,GPIO_PIN_14);
	  xj4[1] = !HAL_GPIO_ReadPin(GPIOG,GPIO_PIN_4);
	  xj4[2] = !HAL_GPIO_ReadPin(GPIOD,GPIO_PIN_15);
	  xj4[3] = !HAL_GPIO_ReadPin(GPIOG,GPIO_PIN_5);
	  xj4[4] = !HAL_GPIO_ReadPin(GPIOG,GPIO_PIN_2);
	  xj4[5] = !HAL_GPIO_ReadPin(GPIOG,GPIO_PIN_6);
	  xj4[6] = !HAL_GPIO_ReadPin(GPIOG,GPIO_PIN_3);
	  xj4[7] = !HAL_GPIO_ReadPin(GPIOG,GPIO_PIN_7);

}

void go_stright()    //直线
{
	//速度


	//循迹
	//led_num = xj2_0 + xj2_1 + xj2_2 + xj2_3 + xj2_4 + xj2_5 + xj2_6 + xj2_7;      //前循�?????????  亮灯个数
//	if(led_num < 4)
//	{

	  error = xj2_0*weight1[0]+xj2_1*weight1[1]+xj2_2*weight1[2]+xj2_3*weight1[3]+xj2_4*weight1[4]+xj2_5*weight1[5]+xj2_6*weight1[6]+xj2_7*weight1[7];
	  //printf("xj0 = %d,error = %d\r\n",xj2_0*weight1[0],error);
	  speed_R = speed - KP*error;
	  speed_L = speed + KP*error;
	//  printf("L :%d   R:%d\r\n",speed_R,speed_L);


	  MOTOR_PWM(2,speed_L);    //左右差�?�控制方�?????????
	  MOTOR_PWM(3,speed_R);    //2号为左电 3为右电机




//	}


}

void go_stright_back()    //直线
{
	//速度


	//循迹
	//led_num = xj2_0 + xj2_1 + xj2_2 + xj2_3 + xj2_4 + xj2_5 + xj2_6 + xj2_7;      //前循�?????????  亮灯个数
//	if(led_num < 4)
//	{

	  error = xj2_0*weight1[0]+xj2_1*weight1[1]+xj2_2*weight1[2]+xj2_3*weight1[3]+xj2_4*weight1[4]+xj2_5*weight1[5]+xj2_6*weight1[6]+xj2_7*weight1[7];
	  //printf("xj0 = %d,error = %d\r\n",xj2_0*weight1[0],error);
	  speed_R = speed - KP*error;
	  speed_L = speed + KP*error;
	//  printf("L :%d   R:%d\r\n",speed_R,speed_L);


	  MOTOR_PWM(3,-speed_L);    //左右差�?�控制方�?????????
	  MOTOR_PWM(2,-speed_R);    //2号为左电 3为右电机




//	}


}



void go_left()
{

	  MOTOR_PWM(2,speed_L%100+turn_speed);
	  MOTOR_PWM(3,speed_R%100+turn_speed);

	  int count2 = __HAL_TIM_GET_COUNTER(&htim2);
	  int count4 = __HAL_TIM_GET_COUNTER(&htim4);


	  int pos = 170;
	  while(abs(count2 - MID_VALUE)<pos || abs(count4 - MID_VALUE)<pos)    //前进到旋转的位置
	  {

		  count2 = __HAL_TIM_GET_COUNTER(&htim2);
		  count4 = __HAL_TIM_GET_COUNTER(&htim4);



		  //=====================循迹===========
		  get_data();     //刷新循迹数据

		  go_stright();

	  }



	  MOTOR_PWM(2,0);
	  MOTOR_PWM(3,0);
//		  HAL_Delay(1000);



	  //编码器复�?????????
	  __HAL_TIM_SET_COUNTER(&htim4,MID_VALUE);    //�?????????      前进减数
	  __HAL_TIM_SET_COUNTER(&htim2,MID_VALUE);	  //�?????????      前进加数


	  MOTOR_PWM(2,0);
	  MOTOR_PWM(3,turn_speed);      //左轮�?????????  右轮�?????????

	  count2 = __HAL_TIM_GET_COUNTER(&htim2);
	  count4 = __HAL_TIM_GET_COUNTER(&htim4);


	  while(abs(count2 - MID_VALUE)<500)    //前进到旋转的位置
	  {

		  //get_data();    //刷新循迹数据
		  count2 = __HAL_TIM_GET_COUNTER(&htim2);
		  count4 = __HAL_TIM_GET_COUNTER(&htim4);
//		  printf("c2 = %d     c4 = %d\r\n",count2,count4);


	  }


//	  MOTOR_PWM(2,0);
//	  MOTOR_PWM(3,0);
//		  HAL_Delay(1000);

	  MOTOR_PWM(2,0);
	  MOTOR_PWM(3,turn_speed);      //左轮�?????????  右轮�?????????


	  int cnt=0;
	  while(abs(count2 - MID_VALUE)<1100 /* && (xj2_3==0) && (xj2_4==0) */)    //前进到旋转的位置
	  {

		  get_data();    //刷新循迹数据



		  if(xj2_3==1||xj2_4==1)
		  {
			  cnt++;

		  }else
		  {
			  cnt = 0;
		  }


		  if(cnt>20)
		  {
			  break;
		  }



		  count2 = __HAL_TIM_GET_COUNTER(&htim2);
		  count4 = __HAL_TIM_GET_COUNTER(&htim4);
//		  printf("c2 = %d     c4 = %d\r\n",count2,count4);


	  }


	  //旋转后的复位。   有bug
	  while(1)
	  {
		  get_data();

//		  printf("%d %d %d %d %d %d %d %d\r\n",xj2_0,xj2_1,xj2_2,xj2_3,xj2_4,xj2_5,xj2_6,xj2_7);

		  error = xj2_0*weight1[0]+xj2_1*weight1[1]+xj2_2*weight1[2]+xj2_3*weight1[3]+xj2_4*weight1[4]+xj2_5*weight1[5]+xj2_6*weight1[6]+xj2_7*weight1[7];


	//	  speed_R = -error;
	//	  speed_L =  error;
	//	//  printf("L :%d   R:%d\r\n",speed_R,speed_L);
	//
	//
	//	  MOTOR_PWM(2,10*speed_L);    //左右差�?�控制方�?????????
	//	  MOTOR_PWM(3,10*speed_R);    //2号为左电 3为右电机
	//
		  if(error>0)
		  {
			  MOTOR_PWM(2,200);    //
			  MOTOR_PWM(3,-200);    //2号为左电 3为右电机
		  }

		  if(error<0)
		  {
			  MOTOR_PWM(2,-200);    //
			  MOTOR_PWM(3,200);    //2号为左电 3为右电机
		  }

		  if(error==0)
		  {
			  MOTOR_PWM(2,0);    //
			  MOTOR_PWM(3,0);    //2号为左电 3为右电机
			  break;
		  }

	  }


	  MOTOR_PWM(2,0);
	  MOTOR_PWM(3,0);
	  //HAL_Delay(1000);


}



void go_right()
{


	  MOTOR_PWM(2,speed_L%100+turn_speed);
	  MOTOR_PWM(3,speed_R%100+turn_speed);

	  int count2 = __HAL_TIM_GET_COUNTER(&htim2);
	  int count4 = __HAL_TIM_GET_COUNTER(&htim4);

	  	 int pos = 170;
	  while(abs(count2 - MID_VALUE)<pos || abs(count4 - MID_VALUE)<pos)    //前进到旋转的位置
	  {

		  count2 = __HAL_TIM_GET_COUNTER(&htim2);
		  count4 = __HAL_TIM_GET_COUNTER(&htim4);



		  //=====================循迹===========
		  get_data();     //刷新循迹数据

		  go_stright();

	  }



	  MOTOR_PWM(2,0);
	  MOTOR_PWM(3,0);
//		  HAL_Delay(1000);



	  //编码器复�?????????
	  __HAL_TIM_SET_COUNTER(&htim4,MID_VALUE);    //�?????????      前进减数
	  __HAL_TIM_SET_COUNTER(&htim2,MID_VALUE);	  //�?????????      前进加数


	  MOTOR_PWM(2,turn_speed);
	  MOTOR_PWM(3,0);      //右轮�?????????  左轮�?????????

	  count2 = __HAL_TIM_GET_COUNTER(&htim2);
	  count4 = __HAL_TIM_GET_COUNTER(&htim4);



	  while(abs(count4 - MID_VALUE)<500)    //前进到旋转的位置
	  {

		  //get_data();    //刷新循迹数据
		  count2 = __HAL_TIM_GET_COUNTER(&htim2);
		  count4 = __HAL_TIM_GET_COUNTER(&htim4);
//		  printf("c2 = %d     c4 = %d\r\n",count2,count4);


	  }

//	  MOTOR_PWM(2,0);
//	  MOTOR_PWM(3,0);
//		  HAL_Delay(1000);

	  MOTOR_PWM(2,turn_speed);
	  MOTOR_PWM(3,0);      //左轮�?????????  右轮�?????????


	  int cnt=0;
	  while(abs(count4 - MID_VALUE)<1100 /* && (xj2_3==0) && (xj2_4==0) */)    //前进到旋转的位置
	  {

		  get_data();    //刷新循迹数据



		  if(xj2_3==1||xj2_4==1)
		  {
			  cnt++;

		  }else
		  {
			  cnt = 0;
		  }


		  if(cnt>20)
		  {
			  break;
		  }



		  count2 = __HAL_TIM_GET_COUNTER(&htim2);
		  count4 = __HAL_TIM_GET_COUNTER(&htim4);
//		  printf("c2 = %d     c4 = %d\r\n",count2,count4);


	  }


//	  MOTOR_PWM(2,0);
//	  MOTOR_PWM(3,0);
//	  HAL_Delay(1000);




	  //旋转后的复位。   有bug
	  while(1)
	  {
		  get_data();

//		  printf("%d %d %d %d %d %d %d %d\r\n",xj2_0,xj2_1,xj2_2,xj2_3,xj2_4,xj2_5,xj2_6,xj2_7);

		  error = xj2_0*weight1[0]+xj2_1*weight1[1]+xj2_2*weight1[2]+xj2_3*weight1[3]+xj2_4*weight1[4]+xj2_5*weight1[5]+xj2_6*weight1[6]+xj2_7*weight1[7];


	//	  speed_R = -error;
	//	  speed_L =  error;
	//	//  printf("L :%d   R:%d\r\n",speed_R,speed_L);
	//
	//
	//	  MOTOR_PWM(2,10*speed_L);    //左右差�?�控制方�?????????
	//	  MOTOR_PWM(3,10*speed_R);    //2号为左电 3为右电机
	//
		  if(error>0)
		  {
			  MOTOR_PWM(2,200);    //
			  MOTOR_PWM(3,-200);    //2号为左电 3为右电机
		  }

		  if(error<0)
		  {
			  MOTOR_PWM(2,-200);    //
			  MOTOR_PWM(3,200);    //2号为左电 3为右电机
		  }

		  if(error==0)
		  {
			  MOTOR_PWM(2,0);    //
			  MOTOR_PWM(3,0);    //2号为左电 3为右电机
			  break;
		  }

	  }

	  MOTOR_PWM(2,0);
	  MOTOR_PWM(3,0);






}


void go_back()
{
//	 //编码器复�?????????
//	__HAL_TIM_SET_COUNTER(&htim4,MID_VALUE);    //�?????????      前进减数
//	__HAL_TIM_SET_COUNTER(&htim2,MID_VALUE);	  //�?????????      前进加数


	MOTOR_PWM(2,turn_speed);
	MOTOR_PWM(3,turn_speed);

	int count2 = __HAL_TIM_GET_COUNTER(&htim2);
	int count4 = __HAL_TIM_GET_COUNTER(&htim4);


	while(abs(count2 - MID_VALUE)<580 || abs(count4 - MID_VALUE)<580)    //前进到旋转的位置
	{

	  count2 = __HAL_TIM_GET_COUNTER(&htim2);
	  count4 = __HAL_TIM_GET_COUNTER(&htim4);



	  //=====================循迹===========
	  get_data();     //刷新循迹数据

	  go_stright();

	}



	MOTOR_PWM(2,0);
	MOTOR_PWM(3,0);
	//		  HAL_Delay(1000);



	//编码器复�?????????
	__HAL_TIM_SET_COUNTER(&htim4,MID_VALUE);    //�?????????      前进减数
	__HAL_TIM_SET_COUNTER(&htim2,MID_VALUE);	  //�?????????      前进加数


	//�?????????个轮子改变方�?????????

	MOTOR_PWM(2,turn_speed);
	MOTOR_PWM(3,-turn_speed);      //右轮�?????????  左轮�?????????





	count2 = __HAL_TIM_GET_COUNTER(&htim2);
	count4 = __HAL_TIM_GET_COUNTER(&htim4);

	//旋转
	while(abs(count2 - MID_VALUE)<800 || abs(count4 - MID_VALUE)<800)    //前进到旋转的位置
	{

	  count2 = __HAL_TIM_GET_COUNTER(&htim2);
	  count4 = __HAL_TIM_GET_COUNTER(&htim4);

	}





	get_data();
	while((xj2_3==0) && (xj2_4==0))    //前进到旋转的位置
	{

	  get_data();    //刷新循迹数据
	  count2 = __HAL_TIM_GET_COUNTER(&htim2);
	  count4 = __HAL_TIM_GET_COUNTER(&htim4);
	  printf("c2 = %d     c4 = %d\r\n",count2,count4);


	}


	MOTOR_PWM(2,0);
	MOTOR_PWM(3,0);
//	HAL_Delay(1000);

}


void go_map(int des_x,int des_y,int des_dir)
{
	int _dir;

	//判断转向    相对坐标
	int _x = des_x - now_xx;
	int _y = des_y - now_yy;

	if(_x>0&&_y>0)  //相对位置在第零象�?????????
	{
		if(now_dirr == 0)
		{
			go_stright_num(_x);
			go_left();
			go_stright_num(_y);
			_dir = 1;
		}
		if(now_dirr == 1)
		{
			go_stright_num(_y);
			go_right();
			go_stright_num(_x);
			_dir = 0;
		}
		if(now_dirr == 2)
		{
			go_right();
			go_stright_num(_y);
			go_right();
			go_stright_num(_x);
			_dir = 0;
		}
		if(now_dirr == 3)
		{
			//=====
			go_left();
			go_stright_num(_x);
			go_left();
			go_stright_num(_y);
			_dir = 1;
		}
	}

	if(_x<0 &&_y>0)
	{
		if(now_dirr == 2)
		{
			go_stright_num(-_x);
			go_right();
			go_stright_num(_y);
			_dir = 1;
		}
		if(now_dirr == 1)
		{
			go_stright_num(_y);
			go_left();
			go_stright_num(_x);
			_dir = 0;
		}
		if(now_dirr == 2)
		{
			go_right();
			go_stright_num(_y);
			go_right();
			go_stright_num(_x);
			_dir = 0;
		}
		if(now_dirr == 3)
		{
			//=====
			go_left();
			go_stright_num(_x);
			go_left();
			go_stright_num(_y);
			_dir = 1;
		}
	}
	if(_x<0 &&_y<0 )_dir = 3;
	if(_x>=0&&_y<0 )_dir = 4;









	MOTOR_PWM(2,0);
	MOTOR_PWM(3,0);
	now_xx = des_x;
	now_yy = des_y;
	now_dirr = _dir;

	//�?????????后将目标状�?�保存为现在状�??
}

void go_stright_num(int num)
{
	while(num)
	{
		count = 0;
		while(1)
		{
			//判断顶线
			get_data();    //刷新循迹数据

			led_num = xj2_0 + xj2_1 + xj2_2 + xj2_3 + xj2_4 + xj2_5 + xj2_6 + xj2_7;      //前循�?????????  亮灯个数

			if(led_num < 4)
			{

			  go_stright();    //走直�?????????

			}

			if(led_num >= 4  && count<20)   //可能到达顶线   不判断位�?????????
			{
			  HAL_Delay(1);
			  count ++;

			  continue;

			}


			if(count>=20)   //判断为顶�?????????
			{
			  count = 0;

			  top_line = 1;

			//编码器复�?????????
			__HAL_TIM_SET_COUNTER(&htim4,MID_VALUE);    //�?????????      前进减数
			__HAL_TIM_SET_COUNTER(&htim2,MID_VALUE);	  //�?????????      前进加数


			  break;
			}



		}

		if(num-1>0)   //�?????????要继续前�?????????
		{
			//编码器复�?????????
			__HAL_TIM_SET_COUNTER(&htim4,MID_VALUE);    //�?????????      前进减数
			__HAL_TIM_SET_COUNTER(&htim2,MID_VALUE);	  //�?????????      前进加数



			MOTOR_PWM(2,speed_L);
			MOTOR_PWM(3,speed_R);

			int count2 = __HAL_TIM_GET_COUNTER(&htim2);
			int count4 = __HAL_TIM_GET_COUNTER(&htim4);


			while(abs(count2 - MID_VALUE)<100 || abs(count4 - MID_VALUE)<100)    //前进到旋转的位置
			{

			  count2 = __HAL_TIM_GET_COUNTER(&htim2);
			  count4 = __HAL_TIM_GET_COUNTER(&htim4);



			  //=====================循迹===========
			  get_data();     //刷新循迹数据

			  go_stright();

			}

			MOTOR_PWM(2,0);
			MOTOR_PWM(3,0);

			//HAL_Delay(1000);



		}



		num--;






	}
}

void go_map1(int now_x,int now_y,int now_dir,int des_x,int des_y,int des_dir)
{

    memcpy(Maze,Maze2,sizeof(Maze2));
    head  = 0;
    rear  = 0;
    level = 2;



    width_flag(des_x,des_y);
    get_path(now_x, now_y);
    print_path();



    //go_stright_num(1);



    int _now_x = flag_queue[1].col,  _now_y=flag_queue[1].row;


    //printf("_x = %d  _y = %d\r\n",_x,_y);


    int dir = 0;

	//判断转向    相对坐标
	int _x = _now_x - now_x;
	int _y = _now_y - now_y;

	printf("_x = %d  _y = %d\r\n",_x,_y);

	if(_x == 1)dir = 0;
	if(_x == -1)dir = 2;
	if(_y == 1)dir = 1;
	if(_y ==-1)dir = 3;


	printf("01-dir = %d",dir);

	if(now_dir-dir == 0)
	{
		//
	}
	if(now_dir-dir == 1||now_dir-dir == -3)
	{
		//右转
		go_right();
		printf("01-right\r\n");
	}
	if(now_dir-dir == 2||now_dir-dir == -2)
	{
		//转半�?????????
		go_back();
		printf("01-back\r\n");

	}
	if(now_dir-dir == -1||now_dir-dir == 3)
	{
		//左转
		go_left();
		printf("01-left\r\n");
	}

	//


	int new_dir;
	for(int i=1; i<level; i++)
	{
		printf("(%d,%d)->", flag_queue[i].col, flag_queue[i].row);

		_x = flag_queue[i].col - flag_queue[i-1].col;
		_y = flag_queue[i].row - flag_queue[i-1].row;

		printf("i= %d   _x = %d  _y = %d\r\n",i,_x,_y);

		if(_x == 1)new_dir = 0;
		if(_x == -1)new_dir = 2;
		if(_y == 1)new_dir = 1;
		if(_y ==-1)new_dir = 3;    //方向


		printf("i=%d new_dir = %d\r\n",i,new_dir);



		if(new_dir == dir)
		{
			//编码器复�?????????
			__HAL_TIM_SET_COUNTER(&htim4,MID_VALUE);    //�?????????      前进减数
			__HAL_TIM_SET_COUNTER(&htim2,MID_VALUE);	  //�?????????      前进加数

			MOTOR_PWM(2,speed_L);
			MOTOR_PWM(3,speed_R);

			int count2 = __HAL_TIM_GET_COUNTER(&htim2);
			int count4 = __HAL_TIM_GET_COUNTER(&htim4);


			while(abs(count2 - MID_VALUE)<100 || abs(count4 - MID_VALUE)<100)    //前进到旋转的位置
			{

			  count2 = __HAL_TIM_GET_COUNTER(&htim2);
			  count4 = __HAL_TIM_GET_COUNTER(&htim4);
			  //=====================循迹===========
			  get_data();     //刷新循迹数据

			  go_stright();

			}

			MOTOR_PWM(2,0);
			MOTOR_PWM(3,0);

			go_stright_num(1);

		}else
		{
			if(new_dir-dir == 0)
			{
				//
			}
			if(dir-new_dir == 1||dir-new_dir == -3)
			{
				//右转
				go_right();
			}
			if(dir-new_dir == 2||dir-new_dir == -2)
			{
				//转半�?????????
				go_back();
			}
			if(dir-new_dir == -1||dir-new_dir == 3)
			{
				//左转
				go_left();
			}

			go_stright_num(1);

		}

		dir = new_dir;

	}




#if 0
		//
		if(des_dir-dir == 0)
		{
			//
		}
		if(dir-des_dir == 1||dir-des_dir == -3)
		{
			//右转
			go_right();
		}
		if(dir-des_dir == 2||dir-des_dir == -2)
		{
			//转半�?????????
			go_back();
		}
		if(dir-des_dir == -1||dir-des_dir == 3)
		{
			//左转
			go_left();
		}
		dir = des_dir;



#endif


	  now_xx  = des_x;
	  now_yy  = des_y;
	  now_dirr = dir;
}
















void get_path(int x, int y)
{
        int i = 0;
        level = Maze[x][y];
        while(level>1)
        {
                flag_queue[i].col = x;
                flag_queue[i].row = y;
                i++;
                level--;
                if(x>0 && Maze[x-1][y]==level)
                        x--;
                else if(y+1<ROW && Maze[x][y+1]==level)
                        y++;
                else if(x+1<COL && Maze[x+1][y]==level)
                        x++;
                else
                        y--;
        }
        level = i;
}
void width_flag(int x, int y)
{
        en_queue(x, y);
        while(queue_not_empty())
        {
                x = flag_queue[head].col;
                y = flag_queue[head].row;
                level = Maze[x][y] + 1;
                if(x>0 && Maze[x-1][y]==0)
                        en_queue(x-1, y);
                if(y+1<ROW && Maze[x][y+1]==0)
                        en_queue(x, y+1);
                if(x+1<COL && Maze[x+1][y]==0)
                        en_queue(x+1, y);
                if(y>0 && Maze[x][y-1]==0)
                        en_queue(x, y-1);
                out_queue();
        }
}
void en_queue(int x, int y)
{
        Maze[x][y] = level;
        flag_queue[rear % (COL * ROW)].col = x;
        flag_queue[rear % (COL * ROW)].row = y;
        rear = (rear + 1) % (COL * ROW);
}
void out_queue()
{
        head = (head + 1) % (COL * ROW);
}
BOOL queue_not_empty()
{
        if(head==rear)
                return FALSE;
        else
                return TRUE;
}
void print_path()
{
        int i;
        printf("\r\nStart->");
        for(i=0; i<level-1; i++)
        {
        	printf("(%d,%d)->", flag_queue[i].col, flag_queue[i].row);



        }





        printf("end\r\n\r\n");
}
void print_maze()
{
        int i,j;
        for(i=0; i<COL; i++)
        {
                for(j=0; j<ROW; j++)
                        printf("%3d",Maze[i][j]);
                printf("\r\n");
        }
}



//串口中断
char class[6]={9};

void HAL_UART_RxCpltCallback(UART_HandleTypeDef *huart)
{
//	if(huart->Instance==USART3)//判断如果�??
//	{
//
//		  //在这里写入自己要实现的代
//		//Rdata[0] = 0;
//		HAL_UART_Receive_IT(&huart3, Rdata, 10);
//
//		HAL_UART_Transmit(&huart3, Rdata, 10,50);
//		//printf("");
//
//		//deep = (Rdata[8]-'0')*100 +(Rdata[9]-'0')*10+(Rdata[10]-'0');
//
//		//printf("12111111111111111111");
//
//
//	}


	if(huart->Instance==USART1)
	{


		//数据在jetson_data内      FF0A9999990AFF



		HAL_UART_Transmit(&huart3, jetson_data, 14,50);

		//无校验
		for(int i=0;i<6;i++)
		{
			class[i] = jetson_data[4+i];
		}





		HAL_UART_Receive_IT(&huart1, jetson_data, 14);

	}

	if(huart->Instance==USART3)
	{



		HAL_UART_Transmit(&huart3, jetson_data, 17,50);

		HAL_UART_Receive_IT(&huart3, jetson_data, 17);

	}


}



////判断动作   return :  动作数    抓取种类
//int action[6][2] = {0};
//
//int get_action()
//{
//
//	//先从中间判断，先拿中间     如果有，中间必有    class[6]   0 1 2 3 4 5
//	int i = 0;
//
//	if(class[1]==)
//
//
//
//}
//



void go_zhua()   //到达目标点顶线   读取nano数据，  需要转向进去抓取，  准备抓物品前的动作   到达定位点
{


	  go_left();
	  HAL_Delay(1000);


	  go_stright_num(1);//转向之后直行1   判断为顶停止。


	  int count2 = __HAL_TIM_GET_COUNTER(&htim2);
	  int count4 = __HAL_TIM_GET_COUNTER(&htim4);
	  while(abs(count2 - MID_VALUE)<200 || abs(count4 - MID_VALUE)<200)    //前进到旋转的位置
	  {

	  	count2 = __HAL_TIM_GET_COUNTER(&htim2);
	  	count4 = __HAL_TIM_GET_COUNTER(&htim4);



	  	//=====================循迹===========
	  	get_data();     //刷新循迹数据

	  	go_stright();

	  }

	  MOTOR_PWM(2,0);
	  MOTOR_PWM(3,0);

	  HAL_Delay(1000);

	  //操作循迹模块
	  while(1)
	  {
		  get_data();

		  printf("%d %d %d %d %d %d %d %d\r\n",xj2_0,xj2_1,xj2_2,xj2_3,xj2_4,xj2_5,xj2_6,xj2_7);

		  error = xj2_0*weight1[0]+xj2_1*weight1[1]+xj2_2*weight1[2]+xj2_3*weight1[3]+xj2_4*weight1[4]+xj2_5*weight1[5]+xj2_6*weight1[6]+xj2_7*weight1[7];

		  if(error>0)
		  {
			  MOTOR_PWM(2,100);    //
			  MOTOR_PWM(3,-100);    //2号为左电 3为右电机
		  }

		  if(error<0)
		  {
			  MOTOR_PWM(2,-100);    //
			  MOTOR_PWM(3,100);    //2号为左电 3为右电机
		  }

		  if(error==0)
		  {
			  MOTOR_PWM(2,0);    //
			  MOTOR_PWM(3,0);    //2号为左电 3为右电机
			  break;
		  }

	  }

	  while(abs(count2 - MID_VALUE)>10 || abs(count4 - MID_VALUE)>10)    //倒退到旋转的位置
	  {


	  	count2 = __HAL_TIM_GET_COUNTER(&htim2);
	  	count4 = __HAL_TIM_GET_COUNTER(&htim4);



	  	//=====================循迹===========
	  	get_data();     //刷新循迹数据

	  	go_stright_back();

	  }

	  MOTOR_PWM(2,0);
	  MOTOR_PWM(3,0);


}










/* USER CODE END 4 */

/**
  * @brief  This function is executed in case of error occurrence.
  * @retval None
  */
void Error_Handler(void)
{
  /* USER CODE BEGIN Error_Handler_Debug */
  /* User can add his own implementation to report the HAL error return state */
  __disable_irq();
  while (1)
  {
  }
  /* USER CODE END Error_Handler_Debug */
}

#ifdef  USE_FULL_ASSERT
/**
  * @brief  Reports the name of the source file and the source line number
  *         where the assert_param error has occurred.
  * @param  file: pointer to the source file name
  * @param  line: assert_param error line source number
  * @retval None
  */
void assert_failed(uint8_t *file, uint32_t line)
{
  /* USER CODE BEGIN 6 */
  /* User can add his own implementation to report the file name and line number,
     ex: printf("Wrong parameters value: file %s on line %d\r\n", file, line) */
  /* USER CODE END 6 */
}
#endif /* USE_FULL_ASSERT */

/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/
